package due.pc.pcshoot.sensor;

import java.util.ArrayList;

import org.anddev.andengine.engine.handler.physics.PhysicsHandler;
import org.anddev.andengine.sensor.accelerometer.AccelerometerData;
import org.anddev.andengine.sensor.accelerometer.IAccelerometerListener;
import org.anddev.andengine.sensor.orientation.IOrientationListener;
import org.anddev.andengine.sensor.orientation.OrientationData;

import android.util.Log;
import due.pc.pcshoot.MainActivity;
import due.pc.pcshoot.control.Objektiv;

/**
 * According to http://en.wikipedia.org/wiki/High-pass_filter and
 * http://en.wikipedia.org/wiki/Low-pass_filter
 * 
 * <p>
 * 
 * code duplication to avoid cost of function call stack switch
 * 
 * @author ZLF
 * 
 */
public class MotionDetector implements IAccelerometerListener,
		IOrientationListener{

	public MotionDetector() {		
		historyX = new ValueHolder(historyCount);
		historyY = new ValueHolder(historyCount);
		historyZ = new ValueHolder(historyCount);
		
		historyA = new ValueHolder(historyCount);
		historyP = new ValueHolder(historyCount);
		historyR = new ValueHolder(historyCount);
	}
	
	public MotionDetector(PhysicsHandler pPhysicsHandler, Objektiv pObjektiv){
		this();
		mPhysicsHandler = pPhysicsHandler;
		mObjektiv = pObjektiv;
	}

	// ===============================================================/
	// Interface Methods
	// ===============================================================/

	public void init(float rate, float freq) {
		float dt = 1.0f / rate;
		float RC = 1.0f / freq;
		filterConstant = dt / (dt + RC);
	}
	
	@Override
	public void onOrientationChanged(OrientationData pOrientationData) {
//		highpassFilterOrientation(pOrientationData.getYaw(), pOrientationData.getPitch(), pOrientationData.getRoll());
//		lowpassFilterOrientation(pOrientationData.getYaw(), pOrientationData.getPitch(), pOrientationData.getRoll());
		
		directOrientation(pOrientationData.getYaw(), pOrientationData.getPitch(), pOrientationData.getRoll());

//		Log.d("MotionDetector", currentOrientation.name()+",\t"+ pOrientationData.getYaw()+",\t\t"+pOrientationData.getPitch()+",\t\t"+pOrientationData.getRoll());
	}

	@Override
	public void onAccelerometerChanged(AccelerometerData pAccelerometerData) {	
		
		highpassFilterAcceleration(pAccelerometerData.getX(), pAccelerometerData.getY(), pAccelerometerData.getZ());
//		lowpassFilterAcceleration(pAccelerometerData.getX(), pAccelerometerData.getY(), pAccelerometerData.getZ());
		
//		historyFilter(pAccelerometerData.getX(),pAccelerometerData.getY(),pAccelerometerData.getZ());
//		historyFilter(highX, highY, highZ);
		
//		float[] velocity = getMotionVelocityPerHistory();
//		mPhysicsHandler.setVelocity(velocity[0] * kAccelerationFactor, velocity[1] * kAccelerationFactor);
//		Log.d("MotionDetector", "" +velocity[0]+", "+velocity[1]);
		
		
	
//		float[] delta = getMotionDeltaPerHistory();
//		mObjektiv.setPosition(mObjektiv.getX()+delta[0], mObjektiv.getY()+delta[1]);
//		Log.d("MotionDetector", ""+delta[0]+", "+delta[1]);
		
		
//		//MuHung			
//		movePerTimeInterval();
		
		updateMotionDeltaPerPhysics();
		
//		float[] velocity = getMotionVelocity();
//		mPhysicsHandler.setVelocity(velocity[0] * kAccelerationFactor, velocity[1] * kAccelerationFactor);
//		Log.d("MotionDetector", "" +velocity[0]+", "+velocity[1]);
		
//		float[] delta = getMotionDelta();
//		mObjektiv.setPosition(mObjektiv.getX()+delta[0], mObjektiv.getY()+delta[1]);
//		Log.d("MotionDetector", ""+delta[0]+", "+delta[1]);
		
//      Log.d("MotionDetector", currentOrientation.name()+", "+pAccelerometerData.getX()+", "+pAccelerometerData.getY()+", "+pAccelerometerData.getZ()+", " +velocity[0]+", "+velocity[1]+", "+delta[0]+", "+delta[1]);
	
	}

	
	
	
	/** get motion velocity */
	public float[] getMotionVelocity(){
		float[] velocity = new float[2];
		switch (currentOrientation) {
		case LEFT:
			velocity[0]=highZ;
			velocity[1]=-highX;
			break;
		case RIGHT:
			velocity[0]=-highZ;
			velocity[1]=-highX;
			break;
		case BOTTOM:
			velocity[0]=highX;
			velocity[1]=-highZ;
			break;
		case TOP:
		default:
			velocity[0]=highX;
			velocity[1]=-highZ;
			break;
		
		}
		return velocity;
	}
	
	/** distance of movements */
	public float[] getMotionDelta(){		
		return distance(getMotionVelocity());
	}
	
	/** get calculated position */
	public float[] getMotionDelatPerPhysics(){
		return new float[]{mPosX,mPosY};
	}
	
	/** get motion delta through history */
	public float[] getMotionVelocityPerHistory(){
		float[] velocity = new float[2];
		
		switch (currentOrientation) {
		case LEFT:
			velocity[0] = historyZ.getMedian(true);
			velocity[1] = -historyX.getMedian(true);
			break;
		case RIGHT:
			velocity[0] = -historyZ.getMedian(true);
			velocity[1] = -historyX.getMedian(true);
			break;
		case BOTTOM:
			velocity[0] = historyX.getMedian(true);
			velocity[1] = -historyZ.getMedian(true);
			break;
		case TOP:
		default:
			velocity[0] = historyX.getMedian(true);
			velocity[1] = -historyZ.getMedian(true);
			break;

		}
		return velocity;
	}
	
	/** get motion delta through history */
	public float[] getMotionDeltaPerHistory(){
		float[] velocity = new float[2];
		float[] max = new float[2];

		switch (currentOrientation) {
		case LEFT:
			velocity[0] = historyZ.getMedian(true);
			velocity[1] = -historyX.getMedian(true);
			max[0] = maxZ;
			max[1] = maxX;
			break;
		case RIGHT:
			velocity[0] = -historyZ.getMedian(true);
			velocity[1] = -historyX.getMedian(true);
			max[0] = maxZ;
			max[1] = maxX;
			break;
		case BOTTOM:
			velocity[0] = historyX.getMedian(true);
			velocity[1] = -historyZ.getMedian(true);
			max[0] = maxX;
			max[1] = maxZ;
			break;
		case TOP:
		default:
			velocity[0] = historyX.getMedian(true);
			velocity[1] = -historyZ.getMedian(true);
			max[0] = maxX;
			max[1] = maxZ;
			break;

		}
		return distancePerHistory(velocity, max);
	}
	
	/** real time sensor data */
	public float[] getMotionVelocityPerRealtime(){
		float[] velocity = new float[2];
		switch (currentOrientation) {
		case LEFT:
			velocity[0]=lastZ;
			velocity[1]=-lastX;
			break;
		case RIGHT:
			velocity[0]=-lastZ;
			velocity[1]=-lastX;
			break;
		case BOTTOM:
			velocity[0]=lastX;
			velocity[1]=-lastZ;
			break;
		case TOP:
		default:
			velocity[0]=lastX;
			velocity[1]=-lastZ;
			break;
		
		}
		
//		velocity[0]=lastX;
//		velocity[1]=lastY;
		return velocity;
	}
	
	/** real time orientation sensor data */
	public float[] getOrientationDeltaPerRealtime(){
		return new float[]{lastA,lastP,lastR};
	}
	
	/** get changes of orientation */
	public float[] getOrientationDelta(){
		return new float[]{deltaA,deltaP,deltaR};
	}
	
	/** get calculated orientation changes */
	public float[] getOrientationDeltaPerHistory(){
		float x = historyA.getMedian(true);
		float y = historyA.getMedian(true);
		float z = historyA.getMedian(true);
		
//		historyA.clear();
//		historyP.clear();
//		historyR.clear();
		
		return new float[]{x,y,z};
	} 
	
	// ===============================================================/
	// Private Constants
	// ===============================================================/

	private static final long UPDATEHANDLER_FIRE_INTERVAL = 100;	//milisekunde
	private static final long UPDATE_MEDIAN_INTERVAL = 20;	//milisekunde
	private static final float SENSORDATA_TO_VELOCITY_FACTOR = 1000;
	
	private boolean adaptive = false;
	private float filterConstant = 0.1f;
	private float kAccelerometerMinStep = 0.02f;
	private float kAccelerometerNoiseAttenuation = 3.0f;
	
	private float kAccelerationFactor = 200;
	private float kDistanceFactor = 10000;
	
	private int historyCount=30;
	
	// ===============================================================/
	// Private Variables
	// ===============================================================/
	private ArrayList<Float[]> mTempMedians = new ArrayList<Float[]>();
	private long mLastPassTime = System.currentTimeMillis();
	private long mLastUpdateMedianTime = System.currentTimeMillis();
	
	private float lastX, lastY, lastZ;
	private float lowX, lowY, lowZ;
	private float highX, highY, highZ;
	private float maxX, maxY, maxZ;
	private long lastTime;

	private float lastA, lastP, lastR;
	private float lowA, lowP, lowR;
	private float highA, highP, highR;	
	private float deltaA, deltaP, deltaR;
	
	private PhysicsHandler mPhysicsHandler;
	private Objektiv mObjektiv;
	
	
	private ValueHolder historyX;
	private ValueHolder historyY;
	private ValueHolder historyZ;
	private ValueHolder historyA;
	private ValueHolder historyP;
	private ValueHolder historyR;

	// ===============================================================/
	// Private Methods
	// ===============================================================/

	private double Norm(float x, float y, float z) {
		return Math.sqrt(x * x + y * y + z * z);
	}

	private double Clamp(double v, double min, double max) {
		if (v > max)
			return max;
		else if (v < min)
			return min;
		else
			return v;
	}

	@SuppressWarnings("unused")
	private void lowpassFilterAcceleration(float x, float y, float z) {
		double alpha = filterConstant;

		if (adaptive) {
			double d = Clamp(Math.abs(Norm(lowX, lowY, lowZ) - Norm(x, y, z))
					/ kAccelerometerMinStep - 1.0, 0.0, 1.0);
			alpha = (1.0 - d) * filterConstant / kAccelerometerNoiseAttenuation
					+ d * filterConstant;
		}

		lowX = (float) (x * alpha + lowX * (1.0 - alpha));
		lowY = (float) (y * alpha + lowY * (1.0 - alpha));
		lowZ = (float) (z * alpha + lowZ * (1.0 - alpha));

		lastX = x;
		lastY = y;
		lastZ = z;
		lastTime = System.currentTimeMillis();
	}

	/** high-pass filter*/
	private void highpassFilterAcceleration(float x, float y, float z) {
		double alpha = filterConstant;

		if (adaptive) {
			double d = Clamp(
					Math.abs(Norm(highX, highY, highZ) - Norm(x, y, z))
							/ kAccelerometerMinStep - 1.0, 0.0, 1.0);
			alpha = d * filterConstant / kAccelerometerNoiseAttenuation
					+ (1.0 - d) * filterConstant;
		}

		highX = (float) (alpha * (highX + x - lastX));
		highY = (float) (alpha * (highY + y - lastY));
		highZ = (float) (alpha * (highZ + z - lastZ));

		lastX = x;
		lastY = y;
		lastZ = z;
		lastTime = System.currentTimeMillis();
	}
	
	/** low-pass filter */
	@SuppressWarnings("unused")
	private void lowpassFilterOrientation(float x, float y, float z) {
		double alpha = filterConstant;

		if (adaptive) {
			double d = Clamp(Math.abs(Norm(lowA, lowP, lowR) - Norm(z, y, z))
					/ kAccelerometerMinStep - 1.0, 0.0, 1.0);
			alpha = (1.0 - d) * filterConstant / kAccelerometerNoiseAttenuation
					+ d * filterConstant;
		}

		lowA = (float) (x * alpha + lowA * (1.0 - alpha));
		lowP = (float) (y * alpha + lowP * (1.0 - alpha));
		lowR = (float) (z * alpha + lowR * (1.0 - alpha));

		if(lastA==0) lastA = x;
		if(lastP==0) lastP = y;
		if(lastR==0) lastR = z;
		
		deltaA = x-lastA;
		deltaP = y-lastP;
		deltaR = z-lastR;
		lastA = x;
		lastP = y;
		lastR = z;
	}

	@SuppressWarnings("unused")
	private void highpassFilterOrientation(float x, float y, float z) {
		double alpha = filterConstant;

		if (adaptive) {
			double d = Clamp(
					Math.abs(Norm(highA, highP, highR) - Norm(x, y, z))
							/ kAccelerometerMinStep - 1.0, 0.0, 1.0);
			alpha = d * filterConstant / kAccelerometerNoiseAttenuation
					+ (1.0 - d) * filterConstant;
		}

		highA = (float) (alpha * (highA + x - lastA));
		highP = (float) (alpha * (highY + y - lastP));
		highR = (float) (alpha * (highR + z - lastR));

		if(lastA==0) lastA = x;
		if(lastP==0) lastP = y;
		if(lastR==0) lastR = z;
		
		deltaA = x-lastA;
		deltaP = y-lastP;
		deltaR = z-lastR;
		lastA = x;
		lastP = y;
		lastR = z;
	}

	
	private void historyFilter(float x, float y, float z){
		historyX.add(x);
		historyY.add(y);
		historyZ.add(z);
		
		maxX = Math.abs(x) > maxX ? Math.abs(x) : maxX;
		maxY = Math.abs(y) > maxY ? Math.abs(y) : maxY;
		maxZ = Math.abs(y) > maxZ ? Math.abs(z) : maxZ;
//		Log.d("MotionDetector",""+maxX+", "+maxY+", "+maxZ);
	}
	
	private Orientation currentOrientation = Orientation.TOP;

	private void directOrientation(float x, float y, float z) {

		if (y < -45 && y > -135) {
			// top side up
			currentOrientation = Orientation.TOP;
		} else if (y > 45 && y < 135) {
			// bottom side up
			currentOrientation = Orientation.BOTTOM;
		} else if (z > 45) {
			// right side up
			currentOrientation = Orientation.RIGHT;
		} else if (z < -45) {
			// left side up
			currentOrientation = Orientation.LEFT;
		}

		if(lastA==0) lastA = x;
		if(lastP==0) lastP = y;
		if(lastR==0) lastR = z;
		
		deltaA = x-lastA;
		deltaP = y-lastP;
		deltaR = z-lastR;
		lastA = x;
		lastP = y;
		lastR = z;
		
	}
	
	private void historyOrientation(float x, float y, float z) {
		directOrientation(x, y, z);
		
		historyA.add(deltaA);
		historyP.add(deltaP);
		historyR.add(deltaR);		
	}
	
	
	private float[] distance(float[] velocity){
		return new float[] {
				(velocity[0] > 0 ? 1 : -1) * Math.min(Math.max(velocity[0], 0.1f), 4) * 10,
				(velocity[0] > 0 ? 1 : -1) * Math.min(Math.max(velocity[1], 0.1f), 4) * 10 };

	}


	private float[] distancePerHistory(float[] velocity, float[] maxVelocity){			
		float[] factor = new float[]{
				Math.abs(velocity[0]) / Math.max(maxVelocity[0], 1.0f),
				Math.abs(velocity[1]) / Math.max(maxVelocity[1], 1.0f) };
		
		
		float [] result =  new float[] {
				(velocity[0] > 0 ? 1 : -1) * Math.min(Math.max(factor[0], 0.1f), 1) * 32,
				(velocity[0] > 0 ? 1 : -1) * Math.min(Math.max(factor[1], 0.1f), 1) * 32 };
		
		Log.d("MotionDetector",""+velocity[0]+", "+velocity[1]+", " + maxVelocity[0]+", "+maxVelocity[1]+", "+factor[0]+", "+factor[1]+",    "+result[0]+", "+result[1]);
		return result;
	}
	
	private void movePerTimeInterval() {
		long elapsedMiSecondsFromLastPassTime = System.currentTimeMillis() - mLastPassTime;
		long elpasedMiSecondsFromLastUpdateTime = System.currentTimeMillis() - mLastUpdateMedianTime;

		if (elpasedMiSecondsFromLastUpdateTime >= UPDATE_MEDIAN_INTERVAL) {
			mTempMedians.add(new Float[] { historyX.getMedian(true) * -1, historyZ.getMedian(true) * -1 });
			mLastUpdateMedianTime = System.currentTimeMillis();
		}

		if (elapsedMiSecondsFromLastPassTime >= UPDATEHANDLER_FIRE_INTERVAL) {
			// probiert
			if (mTempMedians.size() == 0)
				return;
			// ///////

			float[] delta;
			if (mTempMedians.size() == 0) {
				delta = new float[] { historyX.getMedian(true),	historyZ.getMedian(true) };
			} else {
				delta = new float[2];
				delta[0] = 0;
				delta[1] = 0;

				for (int i = 0; i < mTempMedians.size(); i++) {
					delta[0] += mTempMedians.get(i)[0];
					delta[1] += mTempMedians.get(i)[1];
				}
				delta[0] = delta[0] / mTempMedians.size();
				delta[1] = delta[1] / mTempMedians.size();
			}

			delta[0] *= SENSORDATA_TO_VELOCITY_FACTOR;
			delta[1] *= SENSORDATA_TO_VELOCITY_FACTOR;

			// mObjektiv.setPosition(
			// mObjektiv.getX() + delta[0],
			// mObjektiv.getY()+ delta[1]);
			mPhysicsHandler.setVelocity(delta[0] * 150, delta[1] * 150);
//			Log.d("MotionDetector", "" + delta[0] + ", " + delta[1]);

			mLastPassTime = System.currentTimeMillis();
			mTempMedians.clear();
		}
	}
	
	
	// ===============================================================/
	// Private Variables
	// ===============================================================/
    private final float mXDpi = 96.0f;
    private final float mYDpi = 96.0f;
    private final float mMetersToPixelsX = mXDpi / 0.0254f;
    private final float mMetersToPixelsY = mYDpi / 0.0254f;    
    
	private final float mOriginX = (MainActivity.CAMERA_CENTERX-Objektiv.OBJECTIVE_WIDTH)*0.5f;
    private final float mOriginY = (MainActivity.CAMERA_HEIGHT-Objektiv.OBJECTIVE_HEIGHT)*0.5f;
    
    
    private float mPosX;
    private float mPosY;
    private float mLastPosX;
    private float mLastPosY;
    private long  mSensorTimeStamp;
    private float mLastDeltaT;
    private float mAccelX;
    private float mAccelY;
    
    
	
	// ===============================================================/
	// Private Methods
	// ===============================================================/ 
    
    /** according to Accelerometer play example */
	private void updateMotionDeltaPerPhysics() {
		
		
		
		//final long now = mSensorTimeStamp + (System.nanoTime() - mCpuTimeStamp);
		final long now = System.nanoTime();
		float [] sxy = getMotionVelocityPerRealtime();
        final float sx = sxy[0];
        final float sy = sxy[1];
        
        
		final long t = now;        
		if (mSensorTimeStamp != 0) {
            final float dT = (float) (t - mSensorTimeStamp) * (1.0f / 1000000000.0f);
            if (mLastDeltaT != 0) {
                final float dTC = dT / mLastDeltaT;  
                
                // Force of gravity applied to our virtual object
                final float m = 1000.0f; // mass of our virtual object
                final float gx = -sx * m;
                final float gy = -sy * m;

                /*
                 * �F = mA <=> A = �F / m We could simplify the code by
                 * completely eliminating "m" (the mass) from all the equations,
                 * but it would hide the concepts from this sample code.
                 */
                final float invm = 1.0f / m;
                final float ax = gx * invm;
                final float ay = gy * invm;

                /*
                 * Time-corrected Verlet integration The position Verlet
                 * integrator is defined as x(t+�t) = x(t) + x(t) - x(t-�t) +
                 * a(t)�t�2 However, the above equation doesn't handle variable
                 * �t very well, a time-corrected version is needed: x(t+�t) =
                 * x(t) + (x(t) - x(t-�t)) * (�t/�t_prev) + a(t)�t�2 We also add
                 * a simple friction term (f) to the equation: x(t+�t) = x(t) +
                 * (1-f) * (x(t) - x(t-�t)) * (�t/�t_prev) + a(t)�t�2
                 */
                float dTdT = dT * dT;
                mPosX = mObjektiv.getX();
                mPosY = mObjektiv.getY();
                float x = mPosX + (1.0f-filterConstant) * dTC * (mPosX - mLastPosX) + mAccelX * dTdT;
                float y = mPosY + (1.0f-filterConstant) * dTC * (mPosY - mLastPosY) + mAccelY * dTdT;
               
                mLastPosX = mPosX;
                mLastPosY = mPosY;
                mPosX = x;
                mPosY = y;
                mAccelX = ax;
                mAccelY = ay;
                
                
                
                float xx = mOriginX  + mPosX * mMetersToPixelsX;
                float yy = mOriginY - mPosY * mMetersToPixelsY;
//              mObjektiv.setPosition(x,y);
               
//              Log.d("MotionDetector", "-->"+mObjektiv.getX() + ", "+mObjektiv.getY()+", "+mPosX +", "+mPosY+", "+mLastPosX+", "+mLastPosY);            
            }
            mLastDeltaT = dT;
        }
		mSensorTimeStamp = System.nanoTime(); 
	
    }
	
	
}
